// #include <SimpleTimer.h>
#include <PID_v1.h>		//官方pid库
#include "SPI.h"
#include <LiquidCrystal.h>
#include <SimpleTimer.h>
// #include "Ddisplay.h"
//mega interrupt 2,3,4,5


struct kalman  {
    double q;  // process noise covariance
    double r;  // measurement noise covariance ---> error in the measurement
    double x;  // value
    double p;  // estimation error covariance  ---> error in the estimation
    double k;  // kalman gain
    kalman(double q, double r, double p, double intial_value);
    double update(double measurement);
};


kalman::kalman(double init_q, double init_r, double init_p, double init_value) {
    this->q = init_q;
    this->r = init_r;
    this->p = init_p;
    this->x = init_value;
}

double kalman::update(double measurement) {
    this->p = this->p + this->q;


    this->k = this->p / (this->p + this->r);
    this->x = this->x + this->k * (measurement - this->x);
    this->p = ( 1 - this->k ) * this->p;
    
    return this->x;
}



#define lunju 0.675

#define L1 22
#define L2 24
#define LEFT 12		//PWM: 2 to 13 and 44 to 46
#define R1 26
#define R2 28
#define RIGHT 13
// #define COUNT 21	//中断2

#define CSB 53		//ldc1314片选

#define LEN 10		//平滑滤波长度 >2 

#define PID_DELAY_TIME 20

unsigned int timeCount=0;
unsigned long count=0;	
unsigned long timeb;
SimpleTimer t1;
LiquidCrystal lcd(23,25,27,29,31,33);

// Ddisplay myDdisplay;
int std_speed = 90,	//基准速度
	max_speed = 180,
	min_speed = 70;

double Setpoint;		//设定值
double min_set,max_set;

//针对左右轮胎进行不同的pid
double 	
		pwm_input = 0,
		left_output = 0,
		right_output = 0,
		lp = 10,
		li = 0.001,
		ld = 1,
	   	rp = 18,
	   	ri = 0.1,
	   	rd = 1,
	   	sum_left_err = 0,
	   	sum_right_err = 0,
	   	last_left_err = 0,
	   	last_right_err = 0;

long pid_last = 0;


// PID left_pid(&pwm_input, &left_output, &Setpoint,lp,li,ld, DIRECT);
// PID right_pid(&pwm_input, &right_output, &Setpoint,rp,ri,rd, DIRECT);

double distance = 0;	//行走的距离
long last_int_time = 0;//上次中断的时间

int last_input = 0;	//上一次的proximity值

double values[LEN]; 		//平滑滤波使用的数组

int dynamic_setpoint = 0;	//动态基准

// int count = 0;
// 常润清 2016/7/30 2:03:49
//  q r p initial_value.
// q -> process noise.
// r -> sensor noise.
// p -> estimation error covariance.
kalman my_filter(3, 16, 3, 27);

void setup()
{
	Serial.begin(115200);
	pinMode(L1, OUTPUT);
	pinMode(L2, OUTPUT);
	pinMode(R1, OUTPUT);
	pinMode(R2, OUTPUT);
	// pinMode(WOOD, INPUT);

	  // start SPI library/ activate BUS
	SPI.begin();

	pinMode(CSB, OUTPUT); 
	SPI.setBitOrder(MSBFIRST);
	SPI.setDataMode(SPI_MODE0); // CPOL = 0 and CPH = 0 mode 3 also works
	SPI.setClockDivider(SPI_CLOCK_DIV4); // set SCLK @ 4MHz, LDC1000 max is 4MHz DIV2 also works
	
	lcd.begin(16, 2);

	LDC1314_init();
	// LCD1602_init();


	// kalman_init();

	Setpoint += get_filter_value();	//设置初始化的参照值
	Serial.println(Setpoint);
	Setpoint += get_filter_value();	//设置初始化的参照值
	Serial.println(Setpoint);
	Setpoint = Setpoint/2;
	Serial.println(Setpoint);
	show_setPoint(Setpoint);

	min_set = Setpoint;
	max_set = Setpoint;

	t1.setInterval(1000,times);

	timeb=millis();

	// set_setPoint();

	digitalWrite(L1, HIGH);
	digitalWrite(L2, LOW);
	digitalWrite(R1, HIGH);
	digitalWrite(R2, LOW);	

	// left_pid.SetMode(AUTOMATIC);
	// right_pid.SetMode(AUTOMATIC);

	// attachInterrupt(2,calc_distance,RISING);	//上升沿触发中断
	// attachInterrupt(3, setting, RISING);		//not use
	// init_smooth_filter();

	// pid_last = millis();
}

void loop()
{
	// t1.run();
	pwm_input = get_origin_value();
	Serial.println(pwm_input);

	simple_controller_2(pwm_input);

	lcd.setCursor(1, 0);
	lcd.print(pwm_input);

	lcd.setCursor(8, 0);
	lcd.print(pwm_input - Setpoint);
	set_dynamic_setpoint(pwm_input);
	
	// left_pid.Compute();
	// right_pid.Compute();


	// input = map(input,2480,2630,0,50);

	//如何判断是否为直线
	// Serial.print(" ");


	// Serial.print(pwm_input);
	// Serial.print(" ");
	// calc_pid(pwm_input);
	// left_output = calc_left_pin(pwm_input) + std_speed;
	// right_output = calc_right_pin(pwm_input) + std_speed;

	// lcd.setCursor(1, 1);
	// lcd.print(left_output);

	// lcd.setCursor(6, 1);
	// lcd.print(right_output);

	// Serial.print(left_output);
	// Serial.print(" ");
	// Serial.println(right_output);

	// go_on(left_output,right_output);



	// delay(20);
	// go_on(100,130);
}
















// 直接简单判断

void simple_controller_1(int input)
{

	double range_1 = 0.5,
		   range_2 = 1,
		   range_3 = 1.2;

	if(input - Setpoint > range_1/1.7)
	{
		go_on(0,115);
		lcd.setCursor(10, 1);
		lcd.print("RIGHT1");
		if(input - Setpoint > range_2/1.7 )
		{
			go_on(0,125);
			lcd.setCursor(10, 1);
			lcd.print("RIGHT2");
			if(input - Setpoint > range_3/1.7 )
			{
				go_on(0,150);
				lcd.setCursor(10, 1);
				lcd.print("RIGHT3");
			}
		}
	}
	else 
	if(Setpoint - input > range_1/1.1){
		go_on(100,0);
		lcd.setCursor(10, 1);
		lcd.print("LEFT_1");
		if(input - Setpoint > range_2/1.1 )
		{
			go_on(110,0);
			lcd.setCursor(10, 1);
			lcd.print("LEFT_2");
			if(input - Setpoint > range_2/1.1 )
			{
				go_on(115,0);
				lcd.setCursor(10, 1);
				lcd.print("LEFT_3");
			}
		}
	}
	else {
		go_on(80,100);
		lcd.setCursor(10, 1);
		lcd.print("GO   1");
	}


}



//测试,基本没问题
void simple_controller_2(int input)
{

	double range_1 = 0.6,
		   range_2 = 1,
		   range_3 = 1.2;

	if(input - Setpoint > range_1/1.4)
	{
		go_on(86,0);
		lcd.setCursor(10, 1);
		lcd.print("RIGHT1");
		if(input - Setpoint > range_2/1.4 )
		{
			go_on(93,0);
			lcd.setCursor(10, 1);
			lcd.print("RIGHT2");
			if(input - Setpoint > range_3/1.4 )
			{
				go_on(120,0);
				lcd.setCursor(10, 1);
				lcd.print("RIGHT3");
			}
		}
	}
	else 
	if(Setpoint - input > range_1/1.4){
		go_on(0,95);
		lcd.setCursor(10, 1);
		lcd.print("LEFT_1");
		if(input - Setpoint > range_2/1.4)
		{
			go_on(0,105);
			lcd.setCursor(10, 1);
			lcd.print("LEFT_2");
			if(input - Setpoint > range_2/1.4)
			{
				go_on(0,120);
				lcd.setCursor(10, 1);
				lcd.print("LEFT_3");
			}
		}
	}
	else {
		go_on(80,90);
		lcd.setCursor(10, 1);
		lcd.print("GO   1");
	}


}
































void setting()
{
	detachInterrupt(3);
	lcd.setCursor(1, 0);
	lcd.print("Setting:");
	Setpoint = 0;
	Setpoint += get_filter_value();	//设置初始化的参照值
	Serial.println(Setpoint);
	Setpoint += get_filter_value();	//设置初始化的参照值
	Serial.println(Setpoint);
	Setpoint = Setpoint/2;
	Serial.println(Setpoint);
	show_setPoint(Setpoint);
	attachInterrupt(3, setting, RISING);
}


void calc_pid(double input)
{

	if(millis() - pid_last < PID_DELAY_TIME)
		return ;


	double err = input - Setpoint;	//  >0 左偏  左加大

	// sum_left_err += err * li;

	if(sum_right_err > 100)
		sum_right_err = 100;

	double out = lp * err + sum_left_err;	

	if(out > 130)
		out = 130;

	left_output = int(out) + std_speed;

	/////////////////////////////////right

	err = -err*1.3;	

	// sum_right_err += err * ri;	//不会用啊

	if(sum_right_err > 100)
		sum_right_err = 100;

	// double d_err = err - 
 
	out = rp * err + sum_right_err;	

	if(out > 130)
		out = 130;

	right_output = int(out) + std_speed;


	if(right_output > max_speed)
		right_output = max_speed;
	if(left_output > max_speed)
		left_output = max_speed;

	if(right_output < min_speed)
			right_output = min_speed;
	if(left_output < min_speed)
			left_output = min_speed;



	pid_last = millis();
}

//计算并显示距离
void calc_distance()
{
	if((millis()-timeb)>=20){
	    count++;
	    Serial.print("count:");
	    Serial.println(count);
	    lcd.setCursor(4 , 1);
	    // print the number of seconds since reset:
	    lcd.print(count*lunju);
  	}
  	timeb=millis();
}

void LCD1602_init()
{
  lcd.clear();
  lcd.setCursor(1, 0);
  lcd.print("Time:");
  lcd.setCursor(0,1);
  lcd.print("DIS:");
  lcd.setCursor(14,1);
  lcd.print("cm");
}




//带速度
void go_on(int left_pwm,int right_pwm)
{
	// Serial.print("left-  ");
	// Serial.print(left_pwm);
	// Serial.print("right-  ");
	// Serial.println(right_pwm);
	// digitalWrite(L1, HIGH);
	// digitalWrite(L2, LOW);
	// digitalWrite(R1, HIGH);
	// digitalWrite(R2, LOW);	

	analogWrite(LEFT, left_pwm);
	analogWrite(RIGHT, right_pwm);

}

//带速度,后退
void go_on_back(int left_pwm,int right_pwm)
{
	Serial.print("back-  ");
	Serial.println(left_pwm);
	digitalWrite(L1, LOW);
	digitalWrite(L2, HIGH);
	digitalWrite(R1, LOW);
	digitalWrite(R2, HIGH);	

	analogWrite(LEFT, left_pwm);
	analogWrite(RIGHT, right_pwm);

}

void stop()
{
	digitalWrite(L1, LOW);
	digitalWrite(L2, LOW);
	digitalWrite(R1, LOW);
	digitalWrite(R2, LOW );	
}

void start()
{
	digitalWrite(L1, LOW);
	digitalWrite(L2, HIGH);
	digitalWrite(R1, LOW);
	digitalWrite(R2, HIGH);	
}

void slow_down()
{
	std_speed -= 10;
}

void add_speed()
{
	std_speed += 10;
}

void times(){
	
	timeCount++;
	
	lcd.setCursor(7, 0);

	if(timeCount/60>9){

		lcd.print(timeCount/60);
	
	}else{
		lcd.print("0");
		lcd.print(timeCount/60);
	}
	lcd.setCursor(9,0);
	lcd.print("min");
	lcd.setCursor(12,0);

	if(timeCount%60>9){
		lcd.print(timeCount%60);
	}else{
		lcd.print("0");
		lcd.print(timeCount%60);
	}

	lcd.setCursor(14,0);
	lcd.print("s");
}

//原始数据
double get_origin_value() 
{
    unsigned int val = 0;
    unsigned int dataLSB = 0;
    unsigned int dataMSB = 0;
    double proximitydata = 0;

    // Read proximity data LSB register
    digitalWrite(CSB, LOW);
    SPI.transfer(0xA1); // 0x80 + 0x21
    dataLSB = SPI.transfer(0x00);
    digitalWrite(CSB, HIGH);
    delay(5);

    // Read proximity data MSB register
    digitalWrite(CSB, LOW);
    SPI.transfer(0xA2); // 0x80 + 0x22
    dataMSB = SPI.transfer(0x00);
    digitalWrite(CSB, HIGH);
    delay(5);

    proximitydata = ((unsigned int)dataMSB << 8) | (dataLSB);// combine two registers to form 16bit resolution proximity data


	proximitydata = (double)(int(proximitydata / 10))/10;

	// Serial.print(proximitydata);
	// Serial.print(" ");

	proximitydata = my_filter.update(proximitydata);

    return proximitydata ;
}

//设置动态基准
void set_dynamic_setpoint(double new_value)
{
	if((new_value < max_set && new_value > min_set)||(new_value - min_set < 0.3 && new_value - max_set < 0.3))
	{
		double sum = new_value;
		if(new_value > max_set){
			max_set = new_value;
		}
		if (new_value < min_set) {
			min_set = new_value;
		}
	    for(int i=8; i >= 0; i--){
	        sum += values[i];
	        values[i + 1] = values[i];
	    }
	    values[0] = new_value;
	    Setpoint = sum/10;
	    Serial.println(Setpoint);
	}
}


//平滑
double smooth_filter_value()
{
    double ave = 0;
    double sum = 0;
    for(int i=LEN-2; i >= 0; i--){
        sum += values[i];
        values[i + 1] = values[i];
    }
    values[0] = get_origin_value();
    sum += values[0];
    ave = sum / 10;

    // ave = map(ave,5000,6500,50,250);

    // Serial.print(ave);
    // Serial.print(" ");
    // Serial.print(values[0]);
    // Serial.print(" ");

    return ave;

}

void init_smooth_filter()
{
    for(int i=0;i<LEN;i++)
        values[i] = get_origin_value();
}



//初始化
void LDC1314_init()
{
	//Start initialisation of LDC1000
	// set power mode to idle to configure stuff
	digitalWrite(CSB, LOW);
	SPI.transfer(0x0B);
	SPI.transfer(0x00);
	digitalWrite(CSB, HIGH);
	delay(10);

	// Set RpMax
	digitalWrite(CSB, LOW);
	SPI.transfer(0x01);
	SPI.transfer(0x0E);
	digitalWrite(CSB, HIGH);
	delay(10);
	// Set RpMin
	digitalWrite(CSB, LOW);
	SPI.transfer(0x02);
	SPI.transfer(0x3B);
	digitalWrite(CSB, HIGH);
	delay(10);
	// disable all interrupt modes
	digitalWrite(CSB, LOW);
	SPI.transfer(0x0A);
	SPI.transfer(0x00);
	digitalWrite(CSB, HIGH);
	// set thresh HiLSB value
	digitalWrite(CSB, LOW);
	SPI.transfer(0x06);
	SPI.transfer(0x50);
	digitalWrite(CSB, HIGH);
	delay(10);
	// set thresh HiMSB value
	digitalWrite(CSB, LOW);
	SPI.transfer(0x07);
	SPI.transfer(0x14);
	digitalWrite(CSB, HIGH);
	delay(10);
	// set thresh LoLSB value
	digitalWrite(CSB, LOW);
	SPI.transfer(0x08);
	SPI.transfer(0xC0);
	digitalWrite(CSB, HIGH);
	delay(10);
	// set thresh LoMSB value
	digitalWrite(CSB, LOW);
	SPI.transfer(0x09);
	SPI.transfer(0x12);
	digitalWrite(CSB, HIGH);
	delay(10);
	// set power mode to active mode
	digitalWrite(CSB, LOW);
	SPI.transfer(0x0B);
	SPI.transfer(0x01);
	digitalWrite(CSB, HIGH);
	delay(10);
	// end of Initiailsation
}




//初始化的基准值
//先延时2s然后进行取数
void set_setPoint()
{
	// delay(500);
    double sum = 0;
    for(int i=0;i<30;i++)
    {
        // sum  += smooth_filter_value();
        sum  += get_origin_value();
    }

    Setpoint += sum / 30;
    show_setPoint(Setpoint);
}

//去掉最大最小值，然后求平均值
double get_filter_value()
{

    double sum = 0;
    double max,min,tem;
    min = get_origin_value();
    max = min;
    sum = min;
    for(int i=0;i<49;i++)
    {
        tem = get_origin_value();
        if(tem < min)
        {
            min = tem;
        }
        if(tem > max)
        {
            max = tem;
        }
        sum += tem;
    }

    sum = sum - min - max ; //去掉最小最大值

    // int ave = sum / 8;
    double ave = sum / 48;

    show_setPoint(ave);

    return ave;

}



void show_setPoint(double Setpoint)
{
	Serial.print("setpoint:");
    Serial.println(Setpoint);

    lcd.setCursor(1, 0);
	lcd.print("setPoint:");
	lcd.print(Setpoint);


    delay(500);
    LCD1602_init();
}

void kalman_init()
{
	for(int i=0;i<300;i++)
		get_origin_value();
}


